#include "iostream"
#include <ros/ros.h>
#include <vector>
#include "../include/objectPoint.h"
#include "../include/featureObjectExtractor.h"
#include "../include/LSfeatureObject.h"
#include "../include/define.h"
#include "../include/CandidateExtractor.h"
#include <sensor_msgs/LaserScan.h>
#include <geometry_msgs/Pose2D.h>
#include <hri_msgs/Legs.h>

using namespace std;
using namespace hri_msgs;
/*
*   create By: MJ Namazifar(MJ.Namazifar@qiau.ac.ir)
*                R O B O T
*
*                  X
*                  |
*                  |
*                  |
*   -Y <-----------|---------->Y
*     -90           0         90
*
*
*
*              URG or UBG
*       Laser start form right to left
*
*       127 - 640 ---------> 180 ( 90  , -90  )
*       44  - 725 ---------> 240 ( 120 , -120 )
*                    UTM
*
*/


class robina_LegDetector_LRF
{
public:
    robina_LegDetector_LRF():nh("~")
    {
        nh.param<std::string>("topic",scan_topic,"/scan");
        int type;
        nh.param("laser_type",type,0);
        devType = (LRF_DEVICE)type;
        setupLRF(devType);
    }
public:
    void run();
    void laser_sub(const sensor_msgs::LaserScanConstPtr&);
    void poseHuman_pub(vector<Candidate*> pobj);
    void setupLRF(LRF_DEVICE dev);
    void doLegDetection(vector <objectPoint> objps);
private:
    double toRadian(double value);
private:
    ros::Subscriber laserData_sub;
    ros::Publisher  legsPos2D_pub;
    int startIndex , endIndex;
    double startAngle ;
    double endAngle;
    double reso;
    int pointCount;
    int scanCount;
    int SensorFrontStep;
    vector <objectPoint> ObjectPoints;
    featureObjectExtractor featureExtract;
    CandidateExtractor legExtract;
    ros::NodeHandle nh;
    std::string scan_topic;
    LRF_DEVICE devType;

};

double robina_LegDetector_LRF::toRadian(double value)
{
    return value * M_PI / 180;
}
void robina_LegDetector_LRF::setupLRF(LRF_DEVICE dev)
{
    string str = (dev == UTM)? "UTM":"UBG";
    cout<<" Initialized with "<< str <<" Laser Range Finder Device "<<endl;
    startIndex = (dev == UTM)   ? 180 : 128;
    endIndex   =   (dev == UTM) ? 900 : 640;
    pointCount = endIndex - startIndex;
    reso       = 360.0 / ( (dev == UTM) ? 1440 : 1024);
    startAngle = -(pointCount ) * reso / 2.0;
    endAngle   = -startAngle;
    SensorFrontStep = (dev == UTM) ? 540 : 384;

    double t = startAngle;
    cout<<" count point : "<<pointCount<<" startAngle : "<<startAngle<<" EndAngle : "<<endAngle;
    cout<<"\n ---------------------------------------\n";
    for (int i = 0; i < pointCount; i++, t+= reso)
    {
        ObjectPoints.push_back( objectPoint( toRadian(t) , i) );
    }
}


void robina_LegDetector_LRF::run()
{

    laserData_sub = nh.subscribe<sensor_msgs::LaserScan>("/scan",1,
                                                         (boost::function < void(const sensor_msgs::LaserScanConstPtr&)>) boost::bind( &robina_LegDetector_LRF::laser_sub, this, _1 ));
    legsPos2D_pub = nh.advertise<hri_msgs::Legs>("/legs",1);

}
void robina_LegDetector_LRF::laser_sub(const sensor_msgs::LaserScanConstPtr &msg)
{
    if( legsPos2D_pub.getNumSubscribers()==0)
    {
        ros::Rate(10).sleep();
        return;
    }
    for(int i= 0 ; i < msg->ranges.size() ; i++)
    {
        ObjectPoints[i].setR ( msg->ranges[i] * 100);
    }
    doLegDetection(ObjectPoints);
}
void robina_LegDetector_LRF::doLegDetection(vector<objectPoint> objps)
{
    featureExtract.setDiagonal(30,8);
    //(45,14); good
    vector<LSfeatureObject> objects = featureExtract.doExtract( objps , 30 , 12 );//30,8
    //cout<<"\n object count : "<<objects.size();
    legExtract.setObjects(objects);
    //legExtract.doExtractor(2);
    vector<Candidate*> cands = legExtract.doCouple();
    poseHuman_pub(cands);

}

void robina_LegDetector_LRF::poseHuman_pub(vector<Candidate *> candidates)
{
    Legs legs;
    Leg leg;
    //cout<<"\n publish count "<<candidates.size();
    for(vector<Candidate*>::iterator i = candidates.begin();i != candidates.end();i++){
        //  cout<<" \n dist_sep : "<<(*i)->dist_sep<<endl;
        if((*i)->dist_sep == 0 ){
            leg.center_of_leg1.position.x = leg.center_of_leg2.position.x = (*i)->object1_.getMiddlePoint().getX()/100.0;
            leg.center_of_leg1.position.y = leg.center_of_leg2.position.y = (*i)->object1_.getMiddlePoint().getY()/100.0;
            leg.confidence.data = (*i)->object1_.confidenceFactor;
            leg.isPair.data = false;
            legs.legs.push_back(leg);
        }
        else
        {

            if ((*i)->dist_sep <= LEG_SEPARATION){
                leg.center_of_leg1.position.x = (*i)->object1_.getMiddlePoint().getX()/100.0;
                leg.center_of_leg1.position.y = (*i)->object1_.getMiddlePoint().getY()/100.0;
                leg.center_of_leg2.position.x = (*i)->object2_.getMiddlePoint().getX()/100.0;
                leg.center_of_leg2.position.y = (*i)->object2_.getMiddlePoint().getY()/100.0;
                leg.confidence.data =  ((*i)->object1_.confidenceFactor + (*i)->object2_.confidenceFactor) / 2;
                leg.isPair.data = true;
                legs.legs.push_back(leg);
            }
            else
            {
                leg.center_of_leg1.position.x = leg.center_of_leg2.position.x = (*i)->object1_.getMiddlePoint().getX()/100.0;
                leg.center_of_leg1.position.y = leg.center_of_leg2.position.y = (*i)->object1_.getMiddlePoint().getY()/100.0;
                leg.confidence.data = (*i)->object1_.confidenceFactor;
                leg.isPair.data = false;
                legs.legs.push_back(leg);
                //---
                leg.center_of_leg1.position.x = leg.center_of_leg2.position.x = (*i)->object2_.getMiddlePoint().getX()/100.0;
                leg.center_of_leg1.position.y = leg.center_of_leg2.position.y = (*i)->object2_.getMiddlePoint().getY()/100.0;
                leg.confidence.data = (*i)->object2_.confidenceFactor;
                leg.isPair.data = false;
                legs.legs.push_back(leg);
            }
        }
        delete *i;
    }

    legsPos2D_pub.publish(legs);
}

int main(int argc, char *argv[])
{
    ros::init(argc,argv,"roboina_LegDetector_LRF");

    robina_LegDetector_LRF legDetect;
    cout<<"\n LEG DETECTION ==> start"<<endl;
    legDetect.run();
    //    ros::Rate loop(1000);
    //    while(ros::ok())
    //    {
    //        ros::spinOnce();
    //        loop.sleep();
    //    }
    ros::spin();
    cout<<"\n LEG DETECTION ==> finish"<<endl;
    return 0;
}
